#include <Servo.h>

Servo derecha5;
Servo izquierda9;
Servo abajo12;

const int pinButton = 3;
bool isProcessing = false; // Indica si el proceso está en curso

const int servo_ini_i= 175; // ANGULO DE INICIO DE IZQ
const int value_izq = 10; // ANGULO FINAL IZQUIERDA

const int servo_ini_a= 175; // ANGULO DE INICIO DE ABAJO
const int value_abaj = 50; // ANGULO FINAL ABAJO

const int servo_ini_d= 5; // ANGULO DE INICIO DE DERECHA
const int value_der = 170; // ANGULO FINAL DERECHA,


void setup() {
  pinMode(pinButton, INPUT_PULLUP);
  derecha5.attach(5); // Servo motor lado derecho de maquina pin 5 (conector lado derecho de placa)
  izquierda9.attach(9);  // Servo motor del lado izquierdo en pin 9 (conector lado izq de placa)
  abajo12.attach(12);   // Servo motor de abajo en pin 12 (conector medio de placa)
  Serial.begin(74880); // Serial en 74880 se inicializa para empezar el programa
  Serial.println("Initializing");
  delay(1000);

    // Mueve los servos a la posición "standby"
  derecha5.write(servo_ini_d); // Mueve a posición neutral inicio derecho
  izquierda9.write(servo_ini_i); // Mueve a posición neutral izquierdo
  abajo12.write(servo_ini_a);   // Mueve a posicion de inicio abajo
  delay(1000);

  Serial.println("All motors ready"); // Mensaje de aviso de setup listo.

}

void loop() {
  if (!isProcessing) {
    Serial.println("Machine Ready, waiting for button press");
    
    while (digitalRead(pinButton) == HIGH) {
      // Espera aquí hasta que el botón sea presionado
      delay(100); // Pequeña pausa para evitar rebotes
    }

    isProcessing = true;
    Serial.println("Work in progress");

    // Acciones cuando el botón es presionado y el modo es activado
    Serial.println("Right Side");
    derecha5.write(value_der);
    delay(1000);
    derecha5.write(servo_ini_d);
    delay(1000);
    

    Serial.println("Left Side");
    izquierda9.write(value_izq);
    delay(1000);
    izquierda9.write(servo_ini_i);
    delay(1000);

    Serial.println("Bottom");
    abajo12.write(value_abaj);
    delay(1000);
    abajo12.write(servo_ini_a);
    delay(1000); // Final de la rotación de los servo motores
    
    isProcessing = false; // Reinicia el proceso una vez que termina
  }
}